/**
 * @file    run_traj_gene
 * @brief   速度曲线生成
 * @authors 
 */
#include "gene_traj/run_traj_gene.hpp"
using namespace std;
std::ofstream outfile_traj_pub;//生成速度曲线函数
std::ofstream outfile_Ops_sub;//接收定位系统数据
std::ofstream outfile_Max_Acc_state;

fast_planner::FastPlan minco_traj;

Run_Traj_Gene::Run_Traj_Gene() : Node("Traj_Gene")
{
    RCLCPP_INFO(this->get_logger(),"节点Traj_Gene创建");
   //将生成的速度曲线发送给控制模块
    traj_Pub_Custom = this->create_publisher<base_interface::msg::TrajectoryCustom>("traj_generate",10);
    ops_sub_ = this->create_subscription<base_interface::msg::Ops>("/ops_data",10, std::bind(&Run_Traj_Gene::OpsCallback,this,std::placeholders::_1));
    cv_sub_ = this->create_subscription<base_interface::msg::CvData>("Cv_Talker",10, std::bind(&Run_Traj_Gene::CvCallback,this,std::placeholders::_1));
    hit_sub = this->create_subscription<base_interface::msg::Hit>("/cmd_hit",10,std::bind(&Run_Traj_Gene::HitMessageCallback ,this,std::placeholders::_1));
    timer_ = this->create_wall_timer(5ms,std::bind(&Run_Traj_Gene::TimeCallback,this));//创建定时器，每1000ms发布一次消息
  
    //创建输出文本
    std::make_shared<FileOstream>(outfile_traj_pub, "data", "run_traj_gene_txt", "outfile_traj_pub");
    std::make_shared<FileOstream>(outfile_Ops_sub, "data", "run_traj_gene_txt", "Ops_PVA");
    std::make_shared<FileOstream>(outfile_Max_Acc_state, "data", "run_traj_gene_txt", "outfile_Max_Acc_state");

}



auto opsMsg = base_interface::msg::Ops();
Vector2d Start_Pos;//定位系统的位置
Vector2d Start_Vel;//定位系统的速度
Vector2d Start_Acc;//定位系统的加速度

/** 
 * @brief 定位系统回调函数
 */
void Run_Traj_Gene::OpsCallback(const base_interface::msg::Ops::ConstPtr msg)
{

    opsMsg.time_stamp = msg->time_stamp;
    opsMsg.theta = NumDecimal(msg->theta, 4); 
    opsMsg.x     = NumDecimal(msg->x    , 4); 
    opsMsg.y     = NumDecimal(msg->y    , 4); 
    opsMsg.x_vel = NumDecimal(msg->x_vel, 4); 
    opsMsg.y_vel = NumDecimal(msg->y_vel, 4); 
    opsMsg.x_acc = NumDecimal(msg->x_acc, 4);
    opsMsg.y_acc = NumDecimal(msg->y_acc, 4);
    ops_stamp = this->now();

    Start_Pos(0) =  opsMsg.x;
    Start_Pos(1) =  opsMsg.y;
    Start_Vel(0) =  opsMsg.x_vel;
    Start_Vel(1) =  opsMsg.y_vel;
    Start_Acc(0) =  opsMsg.x_acc;
    Start_Acc(1) =  opsMsg.y_acc;

    outfile_Ops_sub<<"Px: "<<"\t"<<Start_Pos(0) <<"\t" <<"Py: "<<"\t" << Start_Pos(1) <<"\t"<< "P_com:" <<"\t" << Start_Pos.norm() <<"\t"<<
                     "Vx: "<<"\t"<<Start_Vel(0) <<"\t" <<"Vy: "<<"\t" << Start_Vel(1) <<"\t"<< "V_com:" <<"\t" << Start_Vel.norm() <<"\t"<<           
                     "Ax: "<<"\t"<<Start_Acc(0) <<"\t" <<"Ay: "<<"\t" << Start_Acc(1) <<"\t"<< "A_com:" <<"\t" << Start_Acc.norm() <<endl;          
}




int act_flag = 1;
int in_count = 0;
int process_flag = 0;
int minco_test_flag = 1;
int maxacc_test_flag = 1;

Vector2d Target_Pos_ture;//定位系统的位置
Vector2d Target_Vel_ture;//定位系统的速度
Vector2d Target_Acc_ture;//定位系统的加速度


uint64_t count_ = 0;
double seconds_;

void Run_Traj_Gene::TimeCallback()//周期为1s
{
count_++;
seconds_ = float(count_) * 0.005;

if(maxacc_test_flag == 0)
{
maxacc_test_flag = 0;

auto node = std::make_shared<rclcpp::Node>("example1_node");
rclcpp::Time time_start = node->now();   

    RCLCPP_INFO(this->get_logger(),"进入gene_traj  第%d次",in_count);//日志输出
    acc_max_Test();

rclcpp::Time time_end = node->now();

printf("生成轨迹所需的总时间是:%f ms", (time_end - time_start).seconds() * 1000);

cout<<endl;

}


if(minco_test_flag == 0)
{
auto node = std::make_shared<rclcpp::Node>("example1_node");

rclcpp::Time time_start = node->now();   

    minco_test_flag = 0;
    in_count++;
    RCLCPP_INFO(this->get_logger(),"进入gene_traj  第%d次",in_count);//日志输出
    MincoTest();

rclcpp::Time time_end = node->now();

printf("生成轨迹所需的总时间是:%f ms", (time_end - time_start).seconds() * 1000);

cout<<endl;

}


if(act_flag == 0)    
{
    in_count++;
    // RCLCPP_INFO(this->get_logger(),"gene_traj 进入定时器 第%d次",in_count);//日志输出
    //  act_flag=0;
// *********************************计时开始********************************
// rclcpp::Time time_start = this->now();

    if(process_flag == 0 && seconds_ >= 1.0){
        count_ = 0;
        act_flag = 0;
        process_flag = 1;
    //--------------------------------------------------------------------------------------------------------------
        base_interface::msg::TrajectoryCustom traj_custom;    /* 切片位置与速度*/
        geometry_msgs::msg::PoseStamped poseStamped;    /* 位置容器*/
        geometry_msgs::msg::Twist velocity;    /* 速度容器*/
        MatrixXd TrajXY_;//定义轨迹切片的接收矩阵

        float angle_set = 90.f/180.f*PI; 
        float length = 0.3;//运动长度
        Target_Pos_ture << Start_Pos(0) + length * cos(angle_set), Start_Pos(1) + length * sin(angle_set);
        Target_Vel_ture << 0.0 , 0.0;
        Target_Acc_ture << 0.0 , 0.0;
        //minco real
        TrajXY_ = minco_traj.MincoPlanManage(0,  Start_Pos,  Start_Vel,  Start_Acc,
                                                Target_Pos_ture, Target_Vel_ture, Target_Acc_ture);                          
            //将切片的位置速度信息装入traj_custom 并发布
            for(int i = 0; i < TrajXY_.rows();i++){
                //填充xy方向的位置
                poseStamped.pose.position.x = TrajXY_(i,0);
                poseStamped.pose.position.y = TrajXY_(i,1);
                //填充xy方向的速度            
                velocity.linear.x = TrajXY_(i,2);
                velocity.linear.y = TrajXY_(i,3);
                //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
                traj_custom.poses.emplace_back(poseStamped);
                traj_custom.velocities.emplace_back(velocity);
                outfile_traj_pub    << "t:" <<"\t"<< 0.005 * i <<"\t"<<
                                    "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
                                    "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
            }
            
            traj_Pub_Custom->publish(traj_custom);
            cout<<"process_flag: "<<process_flag<<endl; 
    }
    // else if(process_flag == 1 && seconds_ >= 0.6){
    //     count_ = 0;
    //     process_flag = 2;

    // //--------------------------------------------------------------------------------------------------------------
    //     base_interface::msg::TrajectoryCustom traj_custom;    /* 切片位置与速度*/
    //     geometry_msgs::msg::PoseStamped poseStamped;    /* 位置容器*/
    //     geometry_msgs::msg::Twist velocity;    /* 速度容器*/
    //     MatrixXd TrajXY_;//定义轨迹切片的接收矩阵
        
    //     float angle_set = 45.f/180.f*PI;
    //     float length = 0.7;//运动长度
    //     Target_Pos_ture << Start_Pos(0) + length * cos(angle_set), Start_Pos(1) + length * sin(angle_set);
    //     Target_Vel_ture << 0.0 , 0.0;
    //     Target_Acc_ture << 0.0 , 0.0;

    //     //minco real
    //     TrajXY_ = minco_traj.MincoPlanManage(0,  Start_Pos,  Start_Vel,  Start_Acc,
    //                                             Target_Pos_ture, Target_Vel_ture, Target_Acc_ture);  
    //         //将切片的位置速度信息装入traj_custom 并发布
    //         for(int i = 0; i < TrajXY_.rows();i++){
    //             //填充xy方向的位置
    //             poseStamped.pose.position.x = TrajXY_(i,0);
    //             poseStamped.pose.position.y = TrajXY_(i,1);
    //             //填充xy方向的速度            
    //             velocity.linear.x = TrajXY_(i,2);
    //             velocity.linear.y = TrajXY_(i,3);
    //             //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
    //             traj_custom.poses.emplace_back(poseStamped);
    //             traj_custom.velocities.emplace_back(velocity);
    //             outfile_traj_pub    << "t:" <<"\t"<< 0.005 * i <<"\t"<<
    //                                 "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
    //                                 "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
    //         }
            
    //         traj_Pub_Custom->publish(traj_custom);
    //         cout<<"process_flag: "<<process_flag<<endl; 
    // }
    // else if(process_flag == 2 /*&& seconds_ >= 0.005*/){
    // process_flag = 999;
    // act_flag = 0;
    // cout<<"process_flag: "<<process_flag<<endl; 
    // }

// //******************************计时结束*************************************
// rclcpp::Time time_end = this->now();
// RCLCPP_INFO(this->get_logger(),"生成以及发布轨迹所需的时间是:%f ms", (time_end - time_start).seconds() * 1000);


}


}



void Run_Traj_Gene::acc_max_Test()
{
// //-----------------------测试用的起点终点状态--------------------------------

    float Tar_x = opsMsg.x + 0.1,Tar_y =opsMsg.y + 0.3;
//-------------------------------------------------------

    MatrixXd TrajXY_;//定义轨迹切片的接收矩阵
    base_interface::msg::TrajectoryCustom traj_custom;    /* 切片位置与速度*/
    geometry_msgs::msg::PoseStamped poseStamped;    /* 位置容器*/
    geometry_msgs::msg::Twist velocity;    /* 速度容器*/


    TrajXY_ = acc_max_trajectory(0.0,  opsMsg.x,  opsMsg.y, 0.0, 0.0, Tar_x, Tar_y, 0.0, 0.0);
                       
    //将切片的位置速度信息装入traj_custom 并发布
    for(int i = 0; i < TrajXY_.rows();i++){
        //填充xy方向的位置
        poseStamped.pose.position.x = TrajXY_(i,0);
        poseStamped.pose.position.y = TrajXY_(i,1);
        //填充xy方向的速度            
        velocity.linear.x = TrajXY_(i,2);
        velocity.linear.y = TrajXY_(i,3);
        //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
        traj_custom.poses.emplace_back(poseStamped);
        traj_custom.velocities.emplace_back(velocity);
        outfile_traj_pub    << "t:" <<"\t"<< 0.005 * i <<"\t"<<
                               "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
                               "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
    }
    outfile_traj_pub << endl;
    
    traj_Pub_Custom->publish(traj_custom);

}


void Run_Traj_Gene::MincoTest()
{
Vector2d Start_Pos_test;//定位系统的位置
Vector2d Start_Vel_test;//定位系统的速度
Vector2d Start_Acc_test;//定位系统的加速度
Vector2d Target_Pos_test;//定位系统的位置
Vector2d Target_Vel_test;//定位系统的速度
Vector2d Target_Acc_test;//定位系统的加速度
//     float angle_set = -70.f/180.f*PI; 
//     float length = 0.5;//运动长度
// //-----------------------测试用的起点终点状态--------------------------------

    Start_Pos_test  << 0.0 ,0.0;
    Start_Vel_test  << 0.0 ,0.0;
    Start_Acc_test  << 0.0 ,0.0;
    // Target_Pos_test << test_x + length * cos(angle_set), test_y + length * sin(angle_set);
    Target_Pos_test << 0.2 , 0.2 ;
    Target_Vel_test << 0.0 , 0.0;
    Target_Acc_test << 0.0 , 0.0;
//-------------------------------------------------------

    base_interface::msg::TrajectoryCustom traj_custom;    /* 切片位置与速度*/
    geometry_msgs::msg::PoseStamped poseStamped;    /* 位置容器*/
    geometry_msgs::msg::Twist velocity;    /* 速度容器*/
    MatrixXd TrajXY_;//定义轨迹切片的接收矩阵

    ////minco_test
    float pose_angle = atan2( (Target_Pos_test(1) - Start_Pos_test(1)), (Target_Pos_test(0) - Start_Pos_test(0)) );
    cout <<"pose_angle: "<< pose_angle/PI*180 <<endl;
    minco_traj.minco_max_acc_ = (acc_compute(pose_angle , 3.5) >= 4.6 ? 4.6 : acc_compute(pose_angle , 3.5));

    TrajXY_ = minco_traj.MincoPlanManage(0,  Start_Pos_test,  Start_Vel_test,  Start_Acc_test,
                                            Target_Pos_test, Target_Vel_test, Target_Acc_test);                          
    //将切片的位置速度信息装入traj_custom 并发布
    for(int i = 0; i < TrajXY_.rows();i++){
        //填充xy方向的位置
        poseStamped.pose.position.x = TrajXY_(i,0);
        poseStamped.pose.position.y = TrajXY_(i,1);
        //填充xy方向的速度            
        velocity.linear.x = TrajXY_(i,2);
        velocity.linear.y = TrajXY_(i,3);
        //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
        traj_custom.poses.emplace_back(poseStamped);
        traj_custom.velocities.emplace_back(velocity);
        outfile_traj_pub    << "t:" <<"\t"<< 0.005 * i <<"\t"<<
                               "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
                               "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
    }
    outfile_traj_pub << endl;
    
    traj_Pub_Custom->publish(traj_custom);

}


int in_traj_plan_flag = 1;
void Run_Traj_Gene::CvCallback(const base_interface::msg::CvData::SharedPtr cv_data)
{   
    // // Eigen::Vector2d DeltaDis;//转到目标角度后的平台与球落点之间的距离 等价于 车需要移动的距离
    // // DeltaDis(0) = cv_data->end_x - ( PlatToCarcenter(0) + Start_Pos(0));
    // // DeltaDis(1) = cv_data->end_y - ( PlatToCarcenter(1) + Start_Pos(1));



    // double Tar_x, Tar_y;

    // // cv_data->delta_time;
    // // Tar_x = Start_Pos(0) + DeltaDis(0);
    // // Tar_y = Start_Pos(1) + DeltaDis(1);

    // if(cv_data->max_z != 0 && in_traj_plan_flag == 1)
    // {
    //     in_traj_plan_flag = 0;

    //     Tar_x = cv_data->end_x - PlatToCarcenter(0);
    //     Tar_y = cv_data->end_y - PlatToCarcenter(1);
    //     cout<<"-----------------------------"<<endl;
    //     cout<<"cv_data->end_x:"<<cv_data->end_x<<endl;
    //     cout<<"cv_data->end_y:"<<cv_data->end_y<<endl;
    //     cout<<"-----------------------------"<<endl;
    //     cout<<"PlatToCarcenter(X):"<<PlatToCarcenter(0)<<endl;
    //     cout<<"PlatToCarcenter(Y):"<<PlatToCarcenter(1)<<endl;
    //     cout<<"-----------------------------"<<endl;
    //     cout<<"Tar_x"<<Tar_x <<endl;
    //     cout<<"Tar_y"<<Tar_y <<endl;
    //     cout<<"-----------------------------"<<endl;

    //     base_interface::msg::TrajectoryCustom traj_custom;    /* 切片位置与速度*/
    //     geometry_msgs::msg::PoseStamped poseStamped;    /* 位置容器*/
    //     geometry_msgs::msg::Twist velocity;    /* 速度容器*/
    //     MatrixXd TrajXY_;//定义轨迹切片的接收矩阵
    //     Target_Pos_ture << Tar_x, Tar_y;
    //     Target_Vel_ture << 0.0 , 0.0;
    //     Target_Acc_ture << 0.0 , 0.0;

    //     //minco real
    //     TrajXY_ = minco_traj.MincoPlanManage(0,  Start_Pos,  Start_Vel,  Start_Acc,
    //                                             Target_Pos_ture, Target_Vel_ture, Target_Acc_ture);  
                                                
    //     //将切片的位置速度信息装入traj_custom 并发布
    //     for(int i = 0; i < TrajXY_.rows();i++){
    //         //填充xy方向的位置
    //         poseStamped.pose.position.x = TrajXY_(i,0);
    //         poseStamped.pose.position.y = TrajXY_(i,1);
    //         //填充xy方向的速度            
    //         velocity.linear.x = TrajXY_(i,2);
    //         velocity.linear.y = TrajXY_(i,3);
    //         //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
    //         traj_custom.poses.emplace_back(poseStamped);
    //         traj_custom.velocities.emplace_back(velocity);
    //         outfile_traj_pub    << "t:" <<"\t"<< 0.005 * i <<"\t"<<
    //                             "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
    //                             "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
    //     }
        
    //     traj_Pub_Custom->publish(traj_custom);

    // }
}


void Run_Traj_Gene::HitMessageCallback(const base_interface::msg::Hit::SharedPtr hit_msg)
{

    if(hit_msg->hit_move_flag == 1)
    {   
        in_count++;
        RCLCPP_INFO(this->get_logger(),"进入gene_traj  第%d次",in_count);//日志输出

        //击球平台的中心点相对于车中心原点的位置-
        Eigen::Vector3d PlatToCarcenter = The_position_of_the_batting_platform(hit_msg->target_pitch_motor, hit_msg->target_roll_motor, 0.0);//hit_msg->target_yaw_motor);
        in_traj_plan_flag = 0;
        double Tar_x = hit_msg->cv_end_x - PlatToCarcenter(0);//NumDecimal( hit_msg->cv_end_x - PlatToCarcenter(0),3);
        double Tar_y = hit_msg->cv_end_y - PlatToCarcenter(1);//NumDecimal( hit_msg->cv_end_y - PlatToCarcenter(1),3);
    

        float lenth_ = sqrt( pow(Tar_y - Start_Pos(1), 2) + pow(Tar_x - Start_Pos(0), 2));
        
        if(lenth_ >= 0.5 || lenth_<0.02){
            cout<<"移动长度为:"<<lenth_<<" 不执行轨迹规划!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
             outfile_traj_pub<<"第"<< in_count << "次, " <<"移动长度为:"<<lenth_<<"  不执行轨迹规划!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
        }
        else
        {
            MatrixXd TrajXY_;//定义轨迹切片的接收矩阵
            base_interface::msg::TrajectoryCustom traj_custom;    /* 切片位置与速度*/
            geometry_msgs::msg::PoseStamped poseStamped;    /* 位置容器*/
            geometry_msgs::msg::Twist velocity;    /* 速度容器*/


            TrajXY_ = acc_max_trajectory(0.0,  opsMsg.x,  opsMsg.y, 0.0, 0.0, Tar_x, Tar_y, 0.0, 0.0);
                            
            //将切片的位置速度信息装入traj_custom 并发布
            for(int i = 0; i < TrajXY_.rows();i++){
                //填充xy方向的位置
                poseStamped.pose.position.x = TrajXY_(i,0);
                poseStamped.pose.position.y = TrajXY_(i,1);
                //填充xy方向的速度            
                velocity.linear.x = TrajXY_(i,2);
                velocity.linear.y = TrajXY_(i,3);
                //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
                traj_custom.poses.emplace_back(poseStamped);
                traj_custom.velocities.emplace_back(velocity);
                outfile_traj_pub    << "t:" <<"\t"<< 0.005 * i <<"\t"<<
                                    "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
                                    "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
            }
            outfile_traj_pub << endl;
            
            traj_Pub_Custom->publish(traj_custom);


        }

    }

}


/*
void Run_Traj_Gene::HitMessageCallback(const base_interface::msg::Hit::SharedPtr hit_msg)
{
    // cv_data->delta_time;

    if(hit_msg->hit_move_flag == 1)
    {   
        in_count++;
        RCLCPP_INFO(this->get_logger(),"进入gene_traj  第%d次",in_count);//日志输出

        //击球平台的中心点相对于车中心原点的位置-
        Eigen::Vector3d PlatToCarcenter = The_position_of_the_batting_platform(hit_msg->target_pitch_motor, hit_msg->target_roll_motor, 0.0);//hit_msg->target_yaw_motor);
        in_traj_plan_flag = 0;


        double Tar_x = hit_msg->cv_end_x - PlatToCarcenter(0);//NumDecimal( hit_msg->cv_end_x - PlatToCarcenter(0),3);
        double Tar_y = hit_msg->cv_end_y - PlatToCarcenter(1);//NumDecimal( hit_msg->cv_end_y - PlatToCarcenter(1),3);
        // outfile_traj_pub<<"hit_msg->cv_end_x: "<<hit_msg->cv_end_x<<" ,hit_msg->cv_end_y: "<<hit_msg->cv_end_y<<endl;
        // outfile_traj_pub<<" PlatToCarcenter(0):"<< PlatToCarcenter(0)<<" , PlatToCarcenter(1): "<< PlatToCarcenter(1)<<endl;
        // outfile_traj_pub<<" Tar_x: "<<Tar_x<<" , Tar_y:"<<Tar_y<<endl;
        // outfile_traj_pub<<"pitch_motor: "<<hit_msg->target_pitch_motor<<", roll_motor: "<<hit_msg->target_roll_motor<<", yaw_motor"<<hit_msg->target_yaw_motor<<endl;
        // outfile_traj_pub<<endl;
        // printf("motor_(pitch, roll, yaw):(%f, %f, %f)\n", hit_msg->target_pitch_motor, hit_msg->target_roll_motor, hit_msg->target_yaw_motor);
        // printf("ball_end(x,y):(%f, %f)\n", hit_msg->cv_end_x, hit_msg->cv_end_y);
        // cout<<"---"<<endl;
        // printf("PlatToCarcenter(x,y):(%f, %f)\n", PlatToCarcenter(0), PlatToCarcenter(1));
        // cout<<"---"<<endl;
        // printf("Tar(x,y):(%f, %f)\n", Tar_x, Tar_y);
        // cout<<"--------------------------------------------------------------------"<<endl;



        float lenth_ = sqrt( pow(Tar_y - Start_Pos(1), 2) + pow(Tar_x - Start_Pos(0), 2));
        
        if(lenth_ >= 0.5 || lenth_<0.03){
            cout<<"移动长度为:"<<lenth_<<" 不执行轨迹规划!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
             outfile_traj_pub<<"第"<< in_count << "次, " <<"移动长度为:"<<lenth_<<"  不执行轨迹规划!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"<<endl;
        }
        else
        {
            base_interface::msg::TrajectoryCustom traj_custom;    // 切片位置与速度
            geometry_msgs::msg::PoseStamped poseStamped;   //  位置容器
            geometry_msgs::msg::Twist velocity;    // 速度容器
            MatrixXd TrajXY_;//定义轨迹切片的接收矩阵
            Target_Pos_ture << Tar_x, Tar_y;
            Target_Vel_ture << 0.0 , 0.0;
            Target_Acc_ture << 0.0 , 0.0;

            //minco real
            float pose_angle = atan2( (Target_Pos_ture(1) - Start_Pos(1)), (Target_Pos_ture(0) - Start_Pos(0)) );
            // cout <<"pose_angle: "<< pose_angle/PI*180 <<endl;
            minco_traj.minco_max_acc_ = (acc_compute(pose_angle , 3.5) >= 4.6 ? 4.6 : acc_compute(pose_angle , 3.5));

            TrajXY_ = minco_traj.MincoPlanManage(0,  Start_Pos,  Start_Vel,  Start_Acc,
                                                    Target_Pos_ture, Target_Vel_ture, Target_Acc_ture);  
                                                    
            //将切片的位置速度信息装入traj_custom 并发布
            for(int i = 0; i < TrajXY_.rows();i++){
                //填充xy方向的位置
                poseStamped.pose.position.x = TrajXY_(i,0);
                poseStamped.pose.position.y = TrajXY_(i,1);
                //填充xy方向的速度            
                velocity.linear.x = TrajXY_(i,2);
                velocity.linear.y = TrajXY_(i,3);
                //将填充好的 poseStamped (位置) 和 twist（速度） 分别添加到 traj_custom 的 poses 和 velocities 容器中
                traj_custom.poses.emplace_back(poseStamped);
                traj_custom.velocities.emplace_back(velocity);
                outfile_traj_pub<< "t:" <<"\t"<< 0.005 * i <<"\t"<<
                                    "x:" <<"\t"<<  TrajXY_(i, 0) <<"\t"<< "y:"  <<"\t"<<  TrajXY_(i, 1) <<"\t"<<
                                    "vx:"<<"\t"<<  TrajXY_(i, 2) <<"\t"<< "vy:" <<"\t"<<  TrajXY_(i, 3) <<endl;      
            }
            
            traj_Pub_Custom->publish(traj_custom);
        }

    }

}
*/



/**
 * @brief 计算击球平台的中心点相对于车中心原点的位置-----------------------------------------------------------------------------------------------------------------
 * @param angle_pitch,angle_roll,angle_yaw: 机构的俯仰 横滚 偏航角
*/
int once_flag_ = 1;
// 击球平面中心相对于车中心原点的x,y,z坐标
Eigen::VectorXd The_position_of_the_batting_platform(float angle_pitch, float angle_roll, float angle_yaw)
{
    float L1 = 110.0;
    float L2 = 186.0;
    float L3 = 68.0;
    float h0 = 195.0;

    float theta3 = angle_pitch/ 180.f * PI;  //俯仰
    float theta2 = angle_roll / 180.f * PI;  //横滚
    float theta1 = angle_yaw  / 180.f * PI;  //偏航
    float theta4 = 0.0;          // 三角梅角度


    // cout<<"******************************************************************************************************************"<<endl;
    //     cout<<"俯仰: "<<theta3 / PI * 180.f<<", 横滚:"<<theta2 / PI * 180.f<<",偏航 : "<<theta1 / PI * 180.f<<endl;

    Eigen::MatrixXd T0_1 = Eigen::MatrixXd::Zero(4, 4);
    Eigen::MatrixXd R0_1 = Eigen::MatrixXd::Zero(4, 4);
    Eigen::MatrixXd T1_2 = Eigen::MatrixXd::Zero(4, 4);
    Eigen::MatrixXd R1_2 = Eigen::MatrixXd::Zero(4, 4);
    Eigen::MatrixXd T2_3 = Eigen::MatrixXd::Zero(4, 4);
    Eigen::MatrixXd R2_3 = Eigen::MatrixXd::Zero(4, 4);

    // Eigen::VectorXd p1_1(4);
    // Eigen::VectorXd p1_0(4);
    // Eigen::VectorXd p2_1(4);
    // Eigen::VectorXd p2_0(4);
    // Eigen::VectorXd p3_1(4);
    Eigen::VectorXd p3_3(4);
    // Eigen::VectorXd v3_1(4);
    // Eigen::VectorXd v3_3(4);

    Eigen::VectorXd p3_0(4);

    float H = h0 + (5.42311682569715E-10) * pow((theta4 * 180.0 / PI), 6) + (-1.93651398665508E-07) * pow((theta4 * 180.0 / PI), 5) + (0.0000302647117567201) * pow((theta4 * 180.0 / PI), 4) + (-0.00264004963371833) * pow((theta4 * 180.0 / PI), 3) + (0.083007456266404) * pow((theta4 * 180.0 / PI), 2) + (2.42443501456533) * (theta4 * 180.0 / PI) + 1.34324368923143;

    T0_1 << 1, 0, 0, 0,
        0, 1, 0, L1,
        0, 0, 1, 0,
        0, 0, 0, 1;

    R0_1 << cos(theta1), -sin(theta1), 0, 0,
        sin(theta1), cos(theta1), 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1;

    T1_2 << 1, 0, 0, 0,
        0, 0, 1, 0,
        0, -1, 0, L2,
        0, 0, 0, 1;

    R1_2 << cos(theta2), -sin(theta2), 0, 0,
        sin(theta2), cos(theta2), 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1;

    T2_3 << 0, 0, -1, 0,
        0, 1, 0, 0,
        1, 0, 0, L3,
        0, 0, 0, 1;

    R2_3 << cos(theta3), -sin(theta3), 0, 0,
        sin(theta3), cos(theta3), 0, 0,
        0, 0, 1, 0,
        0, 0, 0, 1;

    p3_3 << 0, -H, 0, 1;



    
    p3_0 = T0_1 * R0_1 * T1_2 * R1_2 * T2_3 * R2_3 * p3_3;
    
    Eigen::Vector3d plat_to_car_center;
    plat_to_car_center(0) = p3_0(0)/1000.f;
    plat_to_car_center(1) = p3_0(1)/1000.f;
    plat_to_car_center(2) = p3_0(2)/1000.f;



    // cout<<"plat_to_car_center(X):"<<plat_to_car_center(0)<<endl;
    // cout<<"plat_to_car_center(Y):"<<plat_to_car_center(1)<<endl;
    // cout<<"******************************************************************************************************************"<<endl;

    return(plat_to_car_center);


}



//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
//-------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------
/**
 * @brief 各个方向加速度大小计算
 * @param pos_angle 角度
 * @param a_min 最小加速度
*/
//0度方向  最大加速度：4.6m/s²
//45度方向  最大加速度：3.5m/s²
float acc_compute(float pos_angle, float a_min)
{
    float a;

    if(pos_angle >= -PI/4 && pos_angle < PI/4)
    {
        a = a_min*cos(PI/4 - pos_angle) + a_min*cos(PI/4 + pos_angle);
    }
    else if(pos_angle >= PI/4 && pos_angle < PI*3/4)
    {
        a = a_min*cos(PI/4 - (pos_angle-PI/2)) + a_min*cos(PI/4 + (pos_angle-PI/2));
    }
    else if(pos_angle >= PI*3/4 && pos_angle <= PI)
    {
        a = a_min*cos(PI/4 - (pos_angle-PI)) + a_min*cos(PI/4 + (pos_angle-PI));
    }
    else if(pos_angle >= -PI*3/4 && pos_angle < -PI/4)
    {
        a = a_min*cos(PI/4 - (pos_angle+PI/2)) + a_min*cos(PI/4 + (pos_angle+PI/2));
    }
    else if(pos_angle >= -PI && pos_angle < -PI*3/4)
    {
        a = a_min*cos(PI/4 - (pos_angle+PI)) + a_min*cos(PI/4 + (pos_angle+PI));
    }    

    return a;

}

/** 
 * @brief 采用最大加速度规划速度 （此时仅限于初速度为零的情况!!!!!!!!!!!!!!!!!!!）
 * @note （目前仅限于初速度为零的情况!!!!!!!!!!!!!!!!!!!）
 */
MatrixXd acc_max_trajectory(float total_time,   //规划总时常                   
                            float x_current,    //x方向现在的位置           
                            float y_current,    //y方向现在的位置           
                            float x_start_vel,  //x方向开始的速度               
                            float y_start_vel,  //y方向开始的速度           
                            float x_target,     //x方向目标位置                 
                            float y_target,     //y方向目标位置             
                            float x_des_vel,    //x方向达到目标位置的速度   
                            float y_des_vel     //y方向达到目标位置的速度   
                                           )
{
    float pos_angle;//该角度为位置与x轴的夹角
    float pos_combine;//合位置
    float vel_combine,vel_x,vel_y,vel_max;//合速度 x分速度 y分速度 最大速度 平均速度
    float acc_combine,acc_x,acc_y;//合加速度， x分加速度， y分加速度的

    float S;//需要移动的距离
    float a1, a2;//加速阶段加速度 减速阶段加速度
    float t1,T;//加速阶段用时， 总时长

    pos_angle = atan2(y_target -  y_current , x_target - x_current);
    S =  sqrt(pow(x_target - x_current,2) + pow(y_target - y_current,2));

    a1 = (acc_compute(pos_angle , 3.4) >= 4.6 ? 4.6 : acc_compute(pos_angle , 3.4));
    a2 = (acc_compute(pos_angle , 3.4) >= 4.6 ? 4.6 : acc_compute(pos_angle , 3.4));

    T = sqrt(2*S*(a1+a2)/(a1*a2));
    t1 = (a2/(a1+a2))*T;
    vel_max = a1*t1;

    cout<<"T:"<<T<<endl;
    cout<<"pos_angle:"<<pos_angle/PI*180<<endl;
    cout<<" a+:"<<a1<<endl;
    cout<<" a-:"<<a2<<endl;

outfile_Max_Acc_state<< ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>" <<endl;

// outfile_Max_Acc_state<< "时常:\t" << T <<endl;
// outfile_Max_Acc_state<<" x_current:\t "<< x_current <<" \x_target\t"<< x_target<<endl;
// outfile_Max_Acc_state<<" y_current:\t "<< y_current <<" \y_target\t"<< y_target<<endl;
// outfile_Max_Acc_state<< "Vmax:\t" << vel_max <<endl;
// outfile_Max_Acc_state<< "Amax:\t" << a1 <<endl;


outfile_Max_Acc_state<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<" <<endl;
    // cout<<"vel_max:"<<vel_max<<endl;
    MatrixXd Posxy = MatrixXd::Zero(T / delt_t + 1, 4);
    float time = 0.0;
    
    for(int i=0;i<Posxy.rows();i++)
    {
        time = i * delt_t;

        if(time>=0 && time <t1)
        {         
            pos_combine =  0.5*a1*pow(time,2);  
            vel_combine = a1*time;
        }
        else if(time>=t1 && time<=T)
        {
            pos_combine = 0.5*a1*pow(t1,2) + vel_max*(time - t1) - 0.5*a2*pow(time-t1,2);
            vel_combine = vel_max - a2*(time-t1);
        }

        Posxy(i, 0) = pos_combine * cos(pos_angle) + x_current;//x
        Posxy(i, 1) = pos_combine * sin(pos_angle) + y_current;//y
        Posxy(i, 2) = vel_combine * cos(pos_angle);//v_x
        Posxy(i, 3) = vel_combine * sin(pos_angle);//v_y

        // outfile_traj_pub << time <<"\t"<<Posxy(i, 0)<<"\t"<<Posxy(i, 1) <<"\t"<<Posxy(i, 2)<<"\t"<<Posxy(i, 3) << endl;

    }
    return Posxy;
}


/** 
 * @brief 采用正弦曲线规划速度 （此时仅限于初速度为零的情况!!!!!!!!!!!!!!!!!!!）
 * @note （目前仅限于初速度为零的情况!!!!!!!!!!!!!!!!!!!）
 * example：TrajXY_ = sintrajectory( 1.0, opsMsg.x , opsMsg.y , 0, 0, 0.5 * cos(angle_set) , 0.5*sin(angle_set), 0, 0); 
 */
MatrixXd  sintrajectory(float total_time,   //规划总时常       
                        float x_current,    //x方向现在的位置    
                        float y_current,    //y方向现在的位置    
                        float x_start_vel,  //x方向开始的速度    
                        float y_start_vel,  //y方向开始的速度    
                        float x_target,     //x方向目标位置     
                        float y_target,     //y方向目标位置     
                        float x_des_vel,    //x方向达到目标位置的速度
                        float y_des_vel     //y方向达到目标位置的速度    
                                        )
{
    float pos_angle;//该角度为位置与x轴的夹角
    float pos_combine;//合位置
    float vel_combine,vel_x,vel_y,vel_max,vel_average;//合速度 x分速度 y分速度 最大速度 平均速度
    float acc_combine,acc_x,acc_y;//合加速度， x分加速度， y分加速度的
  
    vel_average = sqrt(pow(x_target - x_current,2) + pow(y_target - y_current,2)) / total_time;
    vel_max = 2 * vel_average;
    pos_angle = atan2(y_target -  y_current , x_target - x_current);

    MatrixXd Posxy = MatrixXd::Zero(total_time / delt_t + 1, 6);
    /*eigen矩阵行的序号是0~59 共60行
      eigen矩阵行的序号是0~60 共61行*/
    float time = 0.0;

    cout<<"pos_angle:"<<pos_angle/PI*180.0<<endl;
    
    // outfile_traj_pub << "t" <<"\t"<<"x"<<"\t"<<"y" <<"\t"<<"v_x"<<"\t"<<"v_y"<<"\t"<<"a_x"<<"\t"<<"a_y"<<endl;

    for(int i=0;i<Posxy.rows();i++){
        time = i * delt_t;
        pos_combine = (vel_max/2)*((-total_time/(2*PI))*cos(2*PI/total_time*time - PI/2) + time);
        vel_combine = (vel_max/2)*(sin(2*PI/total_time*time - PI/2) + 1);
        acc_combine = (vel_max/2)*((2*PI/total_time)*cos(2*PI/total_time*time - PI/2));

        Posxy(i, 0) = NumDecimal(pos_combine * cos(pos_angle)+ x_current, 3);//x
        Posxy(i, 1) = NumDecimal(pos_combine * sin(pos_angle)+ y_current, 3);//y
        Posxy(i, 2) = NumDecimal(vel_combine * cos(pos_angle), 3);//v_x
        Posxy(i, 3) = NumDecimal(vel_combine * sin(pos_angle), 3);//v_y
        Posxy(i, 4) = NumDecimal(acc_combine * cos(pos_angle), 3);//a_x
        Posxy(i, 5) = NumDecimal(acc_combine * sin(pos_angle), 3);//a_y
        
        outfile_traj_pub<< "time: "<<"\t"<<time <<"\t"<<Posxy(i, 0)<<"\t"<<Posxy(i, 1) <<"\t"<<Posxy(i, 2)<<"\t"<<Posxy(i, 3)<<"\t"<<Posxy(i, 4)<<"\t"<<Posxy(i, 5)<<endl;

    }
    return Posxy;
}


/*轨迹规划失败,用三次插值曲线插值计算       
/**
 * @brief 三次插值曲线插值计算(当只有起点和终点时，等价为 已知起点和重点及其导数，求解一元三次函数的系数的问题 四个条件求解四个未知数)
*/
MatrixXd cubicSpline(float total_time,  //规划总时常       
                     float x_current,   //x方向现在的位置    
                     float y_current,   //y方向现在的位置    
                     float x_start_vel, //x方向开始的速度    
                     float y_start_vel, //y方向开始的速度    
                     float x_target,    //x方向目标位置     
                     float y_target,    //y方向目标位置     
                     float x_des_vel,   //x方向达到目标位置的速度
                     float y_des_vel    //y方向达到目标位置的速度
                                    )
{
    float x_para_a = x_current;
    float x_para_b = x_start_vel;
    float x_para_c = -1.0 / total_time / total_time * (3.0 * (x_current-x_target) + total_time * (2.0 * x_start_vel + x_des_vel));
    float x_para_d = 1.0 / total_time / total_time / total_time * (2.0 * (x_current-x_target) + total_time * (x_start_vel + x_des_vel));

    float y_para_a = y_current;
    float y_para_b = y_start_vel;
    float y_para_c = -1.0 / total_time / total_time * (3.0 * (y_current-y_target) + total_time * (2.0 * y_start_vel + y_des_vel));
    float y_para_d = 1.0 / total_time / total_time / total_time * (2.0 * (y_current-y_target) + total_time * (y_start_vel + y_des_vel));

    MatrixXd Posxy = MatrixXd::Zero(total_time / delt_t + 1, 6);

    float time = 0.0;

    for (int i = 0; i < Posxy.rows(); i++)
    {
        time = i * delt_t;
        Posxy(i, 0) = x_para_a + x_para_b * time + x_para_c * time * time + x_para_d * time * time * time; //x
        Posxy(i, 1) = y_para_a + y_para_b * time + y_para_c * time * time + y_para_d * time * time * time; //y
        Posxy(i, 2) = x_para_b + 2.0 * x_para_c * time + 3.0 * x_para_d * time * time;   //v_x
        Posxy(i, 3) = y_para_b + 2.0 * y_para_c * time + 3.0 * y_para_d * time * time;   //v_y
        Posxy(i, 4) = 2.0 * x_para_c + 6.0 * x_para_d * time; //a_x
        Posxy(i, 5) = 2.0 * y_para_c + 6.0 * y_para_d * time; //a_y
        outfile_traj_pub<< "time: "<<"\t"<<time <<"\t"<<Posxy(i, 0)<<"\t"<<Posxy(i, 1) <<"\t"<<Posxy(i, 2)<<"\t"<<Posxy(i, 3)<<"\t"<<Posxy(i, 4)<<"\t"<<Posxy(i, 5)<<endl;

    }
    return Posxy;
}
    
// /**
//  * @brief 保留小数点后的数字的个数
//  * @param value 数字
//  * @param n_ 小数点后要保留数字的个数
// */
// float NumDecimal(float value,int n_)
// {
//     return (float)((int)( value * pow(10,n_)))/pow(10,n_);
// }

